.import source "music_macros.asm"

.label num_channels = 5
.label channel_stack_max_depth = 3


 // channel state offsets
.label channel_inst_ptr = 0
.label channel_inst_ptr_lo = 0
.label channel_inst_ptr_hi = 1
.label channel_sid_ptr = 2
.label channel_sid_ptr_lo = 2
.label channel_sid_ptr_hi = 3
.label channel_wait_time = 4
.label channel_wait_time_lo = 4
.label channel_wait_time_hi = 5
.label channel_tempo = 6  // set to zero to deactivate channel
.label channel_stack_ptr = 7
.label channel_stack = 8
.label channel_stack_end = [channel_stack + 2*channel_stack_max_depth]
.label channel_state_size = channel_stack_end

// channel header
.label channel_header_sid_ptr = 0 // two bytes
.label channel_header_size = 2

// oscillator offsets
.label osc_depth_lo = 0
.label osc_depth_hi = 1
.label osc_speed = 2
.label osc_target = 3
.label osc_state = 4

.pc = * "Music channel state"
    
channel_state_array:
    .fill [channel_state_size*num_channels], 0 

.pc = * "Music routines"

// pass the channel id in A, pointer to the channel header in XY.
init_channel:
{
.label channel_id = zero_page_start
.label channel_hdr = zero_page_start + 1 // two bytes
.label channel_state_ptr = zero_page_start + 3 // two bytes
    
    sta channel_id
    stx channel_hdr
    sty channel_hdr+1

    // determine location for our channel state
    :Set16(channel_state_ptr, channel_state_array)
    ldx channel_id
loop:
    beq done
    :AddLit8To16(channel_state_ptr, channel_state_size)
    dex
    jmp loop
done:

    // get sid ptr
    ldy #channel_header_sid_ptr
    lda (channel_hdr),y
    ldy #channel_sid_ptr_lo
    sta (channel_state_ptr),y
    ldy #channel_header_sid_ptr+1
    lda (channel_hdr),y
    ldy #channel_sid_ptr_hi
    sta (channel_state_ptr),y

    // now skip the header
    :AddLit8To16(channel_hdr, channel_header_size)
    
    // store instruction pointer
    lda channel_hdr
    ldy #channel_inst_ptr_lo
    sta (channel_state_ptr),y
    lda channel_hdr+1
    ldy #channel_inst_ptr_hi
    sta (channel_state_ptr),y

    // zero out wait time
    lda #0
    ldy #channel_wait_time_lo
    sta (channel_state_ptr),y
    iny
    sta (channel_state_ptr),y
    
    // set stack pointer to base of channel stack
    lda #channel_stack
    ldy #channel_stack_ptr
    sta (channel_state_ptr),y

    // set to active
    lda #36 // default tempo (TODO)
    ldy #channel_tempo
    sta (channel_state_ptr),y

    rts
}

stop_sid_channel:
{
.label sid_ptr = zero_page_start
    lda #0
    ldy #7
loop:
    dey
    sta (sid_ptr),y
    bne loop

    rts
}
        
.label music_zero_page_work_size = 8

update_channel:
{
.label channel_state = zero_page_start // two bytes
.label instr_ptr = zero_page_start + 2 // two bytes
.label sid_ptr = zero_page_start + 4 // two bytes
.label tmp = zero_page_start + 6
.label sid_ptr_stored = zero_page_start + 7
// NOTE: if adding more state here, update music_zero_page_work_size above.
    
    // check if active
    ldy #channel_tempo
    lda (channel_state),y
    bne is_active
    rts

is_active:
    // burn down wait time
    ldy #channel_wait_time_hi
    lda (channel_state),y
    sta tmp
    dey
    lda (channel_state),y
    ldy #channel_tempo
    sec 
    sbc (channel_state),y
    ldy #channel_wait_time_lo
    sta (channel_state),y
    lda tmp
    sbc #0
    iny
    sta (channel_state),y
    cmp #0
    bmi no_wait
    rts

no_wait:
    lda #0
    sta sid_ptr_stored

    // load instruction ptr
    ldy #channel_inst_ptr_lo
    lda (channel_state),y
    sta instr_ptr
    iny
    lda (channel_state),y
    sta instr_ptr+1

read_instruction:
    // load instruction byte into A&X
    ldy #0
    lda (instr_ptr),y
    tax
    and #%11100000
    bne no_poke

poke:
    // store sid ptr if necessary
    ldy #1
    lda sid_ptr_stored
    bne do_poke
    sty sid_ptr_stored
    ldy #channel_sid_ptr_lo
    lda (channel_state),y
    sta sid_ptr
    iny
    lda (channel_state),y
    sta sid_ptr+1
    ldy #1
    
do_poke:
    // load value and poke into sid (y==1 here)
        lda (instr_ptr),y
    stx tmp
    ldy tmp
        sta (sid_ptr),y

    // next instruction!
    :AddLit8To16(instr_ptr, 2)
    jmp read_instruction

jmp_cmd:
    // load new instruction pointer into AX
    ldy #2
    lda (instr_ptr),y
    tax
    dey
    lda (instr_ptr),y

    // store it and read next instruction
    sta instr_ptr
    stx instr_ptr+1
    jmp read_instruction

jsr_cmd:
    // load stack offset into y
    ldy #channel_stack_ptr
    lda (channel_state),y
    tay

    // store instruction pointer in stack
    lda instr_ptr
    sta (channel_state),y
    iny
    lda instr_ptr+1
    sta (channel_state),y

    // store new stack pointer
    iny
    tya
    ldy #channel_stack_ptr
    sta (channel_state),y

    // now do the same as jmp_cmd
    jmp jmp_cmd

no_poke:
    // parse other instructions here
    txa
    and #%10000000
    bne wait

    txa
    cmp #32
    beq jmp_cmd

    cmp #33
    beq jsr_cmd

    cmp #34
    beq rts_cmd

    cmp #35
    beq tempo_cmd

    // other bytes interpreted as "end"
    lda #0
    ldy #channel_tempo
    sta (channel_state),y
    rts

rts_cmd:
    // reduce stack pointer by 2, load into y
    ldy #channel_stack_ptr
    lda (channel_state),y
    sec
    sbc #2
    sta (channel_state),y
    tay

    // pull instruction pointer from stack and increment by 3
    clc
    lda (channel_state),y
    adc #3
    sta instr_ptr
    iny
    lda (channel_state),y
    adc #0
    sta instr_ptr+1

    jmp read_instruction

tempo_cmd:
    // read a byte, store in the channel tempo byte
    ldy #1
    lda (instr_ptr),y
    ldy #channel_tempo
    sta (channel_state),y

    // next instruction!        
    :AddLit8To16(instr_ptr, 2)
    jmp read_instruction

wait:
    // mask away top bit
    txa
    and #%01111111

    // read 15-bit value
    sta tmp
    ldy #1
    lda (instr_ptr),y

    // add to channel wait time
    ldy #channel_wait_time_lo
    clc
    adc (channel_state),y
    sta (channel_state),y
    iny
    lda tmp
    adc (channel_state),y
    sta (channel_state),y

    // advance instruction pointer
    :AddLit8To16(instr_ptr, 2)

    // store instruction ptr
    ldy #channel_inst_ptr_lo
    lda instr_ptr
    sta (channel_state),y
    iny
    lda instr_ptr+1
    sta (channel_state),y

    rts
}

init_music:
{
    // reset all SID registers to 0
    lda #0
    ldx #25
loop:
    dex
    sta $d400,x
    bne loop

    // master volume to maximum
    lda #15
    sta $d418

    // set channels to inactive
    lda #0
    .for (var i=0; i<num_channels; i++)
    {
       sta channel_state_array + i*channel_state_size + channel_tempo
    }

    // set up interrupt routines
    sei
    lda #<music_interrupt_save_registers
    sta $fffe
    lda #>music_interrupt_save_registers
    sta $ffff
    lda #<music_interrupt
    sta $314
    lda #>music_interrupt
    sta $315

    // set interrupt to occur on raster 100
    lda $d011
    and #%01111111
    sta $d011
    lda #100
    sta $d012
    lda #1
    sta $d01a
    cli
    
    rts
}

music_interrupt_save_area:
    .fill music_zero_page_work_size, 0

music_interrupt_save_registers:
{
    pha
    txa
    pha
    tya
    pha
    // fall through
}
music_interrupt:
{
    // save rom-mapping state, force kernel/io/basic
    lda 1
    pha
    ora #7
    sta 1

    // save zero page work area
    .for (var i = 0; i < music_zero_page_work_size; i++)
    {
        lda [zero_page_start+i]
        sta [music_interrupt_save_area+i]
    }
    
    jsr update_music

    // restore zero page work area
    .for (var i = 0; i < music_zero_page_work_size; i++)
    {
        lda [music_interrupt_save_area+i]
        sta [zero_page_start+i]
    }

    // acknowlegde interrupt
    lda $d019
    sta $d019

    // restore rom-mapping
    pla
    sta 1

    // restore registers
    pla
    tay
    pla
    tax
    pla

    rti
}

update_music:
{
.label channel_state = zero_page_start
    :SaveBorder()
    :SetBorder(5)
    .for (var i=0; i<num_channels; i++)
    {
       lda #<[channel_state_array + i*channel_state_size]
       sta channel_state
       lda #>[channel_state_array + i*channel_state_size]
       sta channel_state+1

       jsr update_channel
    }
    :RestoreBorder()
    rts
}
